#include "stm32regs.h"
#include "core_cm.h"
#include "stm32regbit.h"

#define VECT_TAB_OFFSET 0x00

#define DELAY_MAX 0xFFFFFFFFU
#if !defined(HSE_VALUE)
#define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */
#endif                     /* HSE_VALUE */
#if !defined(HSI_VALUE)
#define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/
#endif                      /* HSI_VALUE */

uint32_t SystemCoreClock = HSI_VALUE;
const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U};
const uint8_t APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U};

void SystemCoreClockUpdate(void)
{
  uint32_t tmp, pllvco, pllr, pllsource, pllm;

  /* Get SYSCLK source -------------------------------------------------------*/
  switch (RCC->CFGR & RCC_CFGR_SWS)
  {
  case 0x04: /* HSI used as system clock source */
    SystemCoreClock = HSI_VALUE;
    break;

  case 0x08: /* HSE used as system clock source */
    SystemCoreClock = HSE_VALUE;
    break;

  case 0x0C: /* PLL used as system clock  source */
    /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN
         SYSCLK = PLL_VCO / PLLR
         */
    pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
    pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U;
    if (pllsource == 0x02UL) /* HSI used as PLL clock source */
    {
      pllvco = (HSI_VALUE / pllm);
    }
    else /* HSE used as PLL clock source */
    {
      pllvco = (HSE_VALUE / pllm);
    }
    pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8);
    pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U;
    SystemCoreClock = pllvco / pllr;
    break;

  default:
    break;
  }
  /* Compute HCLK clock frequency --------------------------------------------*/
  /* Get HCLK prescaler */
  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
  /* HCLK clock frequency */
  SystemCoreClock >>= tmp;
}

void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
  SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2))); /* set CP10 and CP11 Full Access */
#endif

  /* Configure the Vector Table location add offset address ------------------*/
  //__DMB();
#ifdef VECT_TAB_SRAM
  SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#else
  SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
#endif
  //__DSB();
}

__STATIC_INLINE void SYSTICK_Init(uint32_t HCLKFrequency, uint32_t Ticks)
{
  /* Configure the SysTick to have interrupt in 1ms time base */
  SysTick->LOAD = (uint32_t)((HCLKFrequency / Ticks) - 1UL); /* set reload register */
  SysTick->VAL = 0UL;                                        /* Load the SysTick Counter Value */
  SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
                  SysTick_CTRL_TICKINT_Msk |
                  SysTick_CTRL_ENABLE_Msk; /* Enable the Systick Timer */

  NVIC_SetPriority(SysTick_IRQn, 0xFF);
  NVIC_EnableIRQ(SysTick_IRQn);
}

__STATIC_INLINE void SYSTICK_SetClkSource(uint32_t Source)
{
  if (Source == SysTick_CTRL_CLKSOURCE_Msk)
  {
    SET_BIT(SysTick->CTRL, SysTick_CTRL_CLKSOURCE_Msk);
  }
  else
  {
    CLEAR_BIT(SysTick->CTRL, SysTick_CTRL_CLKSOURCE_Msk);
  }
}

void SystemClock_Config(void)
{
#if 1
  u32 tmpreg;
  MODIFY_REG(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_ACR_LATENCY_8WS);
  SET_BIT(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
  tmpreg = READ_BIT(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
  (void)(tmpreg);
  CLEAR_BIT(PWR->CR5, PWR_CR5_R1MODE);
  CLEAR_BIT(RCC->APB1ENR1, RCC_APB1ENR1_PWREN);
  SET_BIT(RCC->CR, RCC_CR_HSEON); //打开外部高速时钟
                                  //while (((READ_BIT(RCC->CR, RCC_CR_HSERDY) == (RCC_CR_HSERDY)) ? 1UL : 0UL) != 1) //等待启动
  while ((RCC->CR & RCC_CR_HSERDY) != 0)
    ;
  MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLLSRC | RCC_PLLCFGR_PLLM | RCC_PLLCFGR_PLLN | RCC_PLLCFGR_PLLR,
             RCC_PLLCFGR_PLLSRC_HSE | (RCC_PLLCFGR_PLLM_2 | RCC_PLLCFGR_PLLM_0) | (85 << RCC_PLLCFGR_PLLN_Pos) | 0);
  MODIFY_REG(RCC->PLLCFGR, RCC_PLLCFGR_PLLSRC | RCC_PLLCFGR_PLLM | RCC_PLLCFGR_PLLN | RCC_PLLCFGR_PLLPDIV,
             RCC_PLLCFGR_PLLSRC_HSE | (RCC_PLLCFGR_PLLM_2 | RCC_PLLCFGR_PLLM_0) | (85 << RCC_PLLCFGR_PLLN_Pos) | (RCC_PLLCFGR_PLLPDIV_4 | RCC_PLLCFGR_PLLPDIV_3 | RCC_PLLCFGR_PLLPDIV_0));
  SET_BIT(RCC->PLLCFGR, RCC_PLLCFGR_PLLREN);
  SET_BIT(RCC->PLLCFGR, RCC_PLLCFGR_PLLPEN);

  SET_BIT(RCC->CR, RCC_CR_PLLON);
  /* Wait till PLL is ready */
  //while (((READ_BIT(RCC->CR, RCC_CR_PLLRDY) == (RCC_CR_PLLRDY)) ? 1UL : 0UL) != 1)
  while ((RCC->CR & RCC_CR_PLLRDY) != 0)
    ;
  MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_PLL);
  MODIFY_REG(RCC->CFGR, RCC_CFGR_HPRE, RCC_CFGR_HPRE_DIV2);
  while (((uint32_t)(READ_BIT(RCC->CFGR, RCC_CFGR_SWS))) != RCC_CFGR_SWS_PLL)
    ;
  //todo set CoreDebug->DEMCR
  //CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
  DWT->CYCCNT = 0;
  while (DWT->CYCCNT < 100)
    ;

  MODIFY_REG(RCC->CFGR, RCC_CFGR_HPRE, RCC_CFGR_HPRE_DIV1);
  MODIFY_REG(RCC->CFGR, RCC_CFGR_PPRE1, RCC_CFGR_PPRE1_DIV1);
  MODIFY_REG(RCC->CFGR, RCC_CFGR_PPRE2, RCC_CFGR_PPRE2_DIV2);
#endif
  SystemCoreClock = 170000000;
  SYSTICK_Init(SystemCoreClock, 1000U);
  SYSTICK_SetClkSource(SysTick_CTRL_CLKSOURCE_Msk);
}

void System_mDelay(uint32_t Delay)
{
  __IO uint32_t tmp = SysTick->CTRL; /* Clear the COUNTFLAG first */
  uint32_t tmpDelay;                 /* MISRAC2012-Rule-17.8 */
  /* Add this code to indicate that local variable is not used */
  ((void)tmp);
  tmpDelay = Delay;
  /* Add a period to guaranty minimum wait */
  if (tmpDelay < DELAY_MAX)
  {
    tmpDelay++;
  }

  while (tmpDelay != 0U)
  {
    if ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) != 0U)
    {
      tmpDelay--;
    }
  }
}
